/*********************************************************** 
*  --- OpenSURF ---                                        *
*  This library is distributed under the GNU GPL. Please   *
*  contact chris.evans@irisys.co.uk for more information.  *
*                                                          *
*  C. Evans, Research Into Robust Visual Features,         *
*  MSc University of Bristol, 2008.                        *
*                                                          *
************************************************************/

#include "surflib.h"
#include "kmeans.h"
#include <ctime>
#include <iostream>
#include <cmath>
#include <fstream>
#include <ctime> 
//-------------------------------------------------------
// In order to you use OpenSURF, the following illustrates
// some of the simple tasks you can do.  It takes only 1
// function call to extract described SURF features!
// Define PROCEDURE as:
//  - 1 and supply image path to run on static image
//  - 2 to capture from a webcam
//  - 3 to match find an object in an image (work in progress)
//  - 4 to display moving features (work in progress)
//  - 5 to show matches between static images
#define PROCEDURE 3

//-------------------------------------------------------

int mainImage(void);
int mainVideo(void);
int mainMatch(void);
int mainMotionPoints(void);
int mainStaticMatch(void);
int mainKmeans(void);

//------------------------------------------------------------------------------------
struct newVars{  	//Struct to hold variables for CompleteOps method (for center and radius calculations)

  int Total; 		//the total number of matches if matches.size >= 6
  int centerX; 		//current center x-coordinate
  int centerY;		//current center y-coordinate
  int LX;		//left most x-coordinate in a group of matches
  int RX;		//right most x-coordinate in a group of matches
  int BY;		//bottom most y-coordinate in a group of matches
  int TY;		//top most y-coordinate in a group of matches
  int radius; 		//current radius
  int radiusHold; 	//holds up to 10 consecutive radius values (then averages them on the 10th to use as the final radisus -- see radiusFinal)
  int circleCount; 	//variable that counts to 10-- used for averaging radius
  int radiusFinal;	//the average radius
  int centerX_Hold;	//holds up to 10 consecutive center-x values (then averages them on the 10th to use as the final center coordinate -- see centerX_Fin)
  int centerY_Hold;	//holds up to 10 consecutive center-y values (then averages them on the 10th to use as the final center coordinate -- see centerY_Fin)
  int centerX_Fin;	//the average center-x coordinate
  int centerY_Fin;	//the average center-y coordinate
  
  double b; 
  double percent; 
  double a; //a = Tan(24.6236 degrees) which is 1/2 the particular viewing angle of this camera
  char dist[20];
  double bhold;
  double counter;
  double counterCenter;
};

struct newVars st_vars; 
//-------------------------------------------------------
//*****CompleteOps is the replacement method for the main operations performed on matches,matches2,and matches3. ********
void CompleteOps(IpPairVec matches, struct newVars *csun, IplImage* img, CvFont* font){
	 //Here we find center point of matches array,
        csun->Total = matches.size(); 
	csun->LX = matches[0].first.x; //set first value to be extremes for all four. 
	csun->RX = matches[0].first.x; 
	csun->TY = matches[0].first.y;
	csun->BY = matches[0].first.y;
	for(int i = 1;i<csun->Total;i++){ //step through vector, find maximum and minimum x points
		if(matches[i].first.x < csun->LX) csun->LX = matches[i].first.x;
		if(matches[i].first.x > csun->RX) csun->RX = matches[i].first.x; 
	}
	for(int j = 1;j<csun->Total;j++){ //same as x, but for y min,max
		if(matches[j].first.y > csun->BY) csun->BY = matches[j].first.y;
		if(matches[j].first.y < csun->TY) csun->TY = matches[j].first.y;
	}
	csun->centerX = (csun->RX + csun->LX)/2; //average x points to find center
	csun->centerY = (csun->BY + csun->TY)/2; //average y points 
	std::cout<<"Center Coordinates = "<<csun->centerX<< " , "<<csun->centerY<<"\n";
	
	//sends serial commands (to adjust servos) to arduino based on where on the screen the center coordinates are
	int check2=0,check3=0,check4=0,check5=0,check6=0,check7=0,check8=0,check9=0; //serial command checks
	if(csun->centerX<775 && csun->centerX>=725)check2 = system("./../arduino/arduino -b 9600 -p /dev/ttyUSB0 -s 2"); //go left soft
	else if(csun->centerX>825 && csun->centerX <=875)check3 = system("./../arduino/arduino -b 9600 -p /dev/ttyUSB0 -s 3"); //go right soft
	if(csun->centerX<725)check6 = system("./../arduino/arduino -b 9600 -p /dev/ttyUSB0 -s 7"); //go right hard
	else if(csun->centerX>875)check7 = system("./../arduino/arduino -b 9600 -p /dev/ttyUSB0 -s 6"); //go left hard
	
	if(csun->centerY<675 && csun->centerY>=625)check4 = system("./../arduino/arduino -b 9600 -p /dev/ttyUSB0 -s 4"); //go up soft
	else if(csun->centerY>525 && csun->centerY<=575)check5 = system("./../arduino/arduino -b 9600 -p /dev/ttyUSB0 -s 5"); //go down soft
	if(csun->centerY<525)check6 = system("./../arduino/arduino -b 9600 -p /dev/ttyUSB0 -s 9"); //go down hard
	else if(csun->centerY>675)check7 = system("./../arduino/arduino -b 9600 -p /dev/ttyUSB0 -s 8"); //go up hard
	if(check2||check3||check4||check5||check6||check7||check8||check9)					 //if there are any errors in the system call...
		std::cout<<"Serial Error"<<check2<<check3<<check4<<check5<<check6<<check7<<check8<<check9<<'\n'; //output error in serial comm
	
	
        csun->centerX_Hold += csun->centerX; //collect multiple Xs and Ys
	csun->centerY_Hold += csun->centerY;
	csun->counterCenter++;
	if(csun->counterCenter>10){ //average 10 values for X and Y, then clear counters
	   csun->centerX_Fin = (csun->centerX_Hold / csun->counterCenter);
	   csun->centerY_Fin = (csun->centerY_Hold / csun->counterCenter);
	   csun->counterCenter = 0;	//reset values
	   csun->centerX_Hold = 0;
	   csun->centerY_Hold = 0;
	}
        //Here we find radius of circle (half length of object), and draw circle around object.     
        for(int i = 0; i<csun->Total; i++){//for all matches in the array...
    	   double x = pow( (fabs( (double) (csun->centerX-matches[i].first.x) ) ),2);	//take |centerx - x|^2
    	   double y = pow( (fabs( (double) (csun->centerY-matches[i].first.y) ) ),2);	//take |centery - y|^2
    	   int mag = (int) sqrt(x + y);			//do magnitude using the two above terms
    	   if(mag > csun->radius)	//if the current magnitude is greater than the radius, replace the radius with the current magnitude	
    		csun->radius = mag;
        }
        csun->radiusHold += csun->radius;	//hold the radius for 10 
        csun->circleCount++;
        if(csun->circleCount>10){ //by adding counter, we can collect an average of radius sizes, to avoid that ugly fluctuation we experience otherwise
           csun->radiusFinal = (csun->radiusHold/csun->circleCount);
           csun->circleCount = 0;
           csun->radiusHold = 0;
        }
        cvCircle(img, cvPoint(csun->centerX,csun->centerY),csun->radiusFinal, cvScalar(0,0,255), 2,8,0); //draw circle around object
	
        //section to attempt to determine relative distance from camera.
       	csun->percent = (csun->radius/176); // 352 is width of screen (width of viewable area) but we're working with half screen, so use 320
        csun->b = ((4.25/csun->percent)/csun->a); // b = distance to detected object security compound or chief of security. use 2.5 for ministry of torture *ADJUSTED*
	csun->bhold += csun->b; //cumulative add of b
	csun->counter++;
	if(csun->counter>10){ //to smooth distance by averaging over ten+ iterations. just go with it.
	   csun->b = (csun->bhold/csun->counter);
	   sprintf(csun->dist,"Distance = %.2f",csun->b);
	   csun->bhold = 0;
	   csun->counter = 0;
	}
	//cvPutText(img,csun->dist, cvPoint(10,110), font, cvScalar(255,0,0)); //declare distance on screen. 
/*        
        if(csun->centerX <= (640/2) && csun->centerY <= (640/2)) 
              csun->cvPutText (img, "Quadrant 1", cvPoint(10,80), &font, cvScalar(255,255,0));
	if(csun->centerX <= (640) && csun->centerY <= (640/2) && csun->centerX > 320 && csun->centerY > 0)
              cvPutText (img, "Quadrant 2", cvPoint(10,80), &font, cvScalar(255,255,0));
        if(csun->centerX <= (640) && csun->centerY <= (640) && csun->centerX > 320 && csun->centerY >320)
              cvPutText (img, "Quadrant 3", cvPoint(10,80), &font, cvScalar(255,255,0));
        if(csun->centerX <= (640/2) && csun->centerY <= 640 && csun->centerX > 0 && csun->centerY > 320)
              cvPutText (img, "Quadrant 4", cvPoint(10,80), &font, cvScalar(255,255,0)); */
}

//----------------------------------------------------

int main(void) 
{
  if (PROCEDURE == 1) return mainImage();
  if (PROCEDURE == 2) return mainVideo();
  if (PROCEDURE == 3) return mainMatch();
  if (PROCEDURE == 4) return mainMotionPoints();
  if (PROCEDURE == 5) return mainStaticMatch();
  if (PROCEDURE == 6) return mainKmeans();
}


//-------------------------------------------------------

int mainImage(void)
{
  // Declare Ipoints and other stuff
  IpVec ipts;
  IplImage *img=cvLoadImage("Images/Cheif_of_Security.jpg");

  // Detect and describe interest points in the image
  clock_t start = clock();
  surfDetDes(img, ipts, false, 5, 4, 2, 0.0004f); 
  clock_t end = clock();

  std::cout<< "OpenSURF found: " << ipts.size() << " interest points" << std::endl;
  std::cout<< "OpenSURF took: " << float(end - start) / CLOCKS_PER_SEC  << " seconds" << std::endl;

  // Draw the detected points
  drawIpoints(img, ipts);
  
  // Display the result
  showImage(img);

  return 0;
}


//-------------------------------------------------------


int mainVideo(void)
{
  // Initialise capture device
  CvCapture* capture = cvCaptureFromCAM( CV_CAP_ANY );
  if(!capture) error("No Capture");

  // Create a window 
  cvNamedWindow("OpenSURF", CV_WINDOW_AUTOSIZE );

  // Declare Ipoints and other stuff
  IpVec ipts;
  IplImage *img=NULL;

  // Main capture loop
  while( 1 ) 
  {
    // Grab frame from the capture source
    img = cvQueryFrame(capture);

    // Extract surf points
    surfDetDes(img, ipts, true, 3, 4, 2, 0.004f);    

    // Draw the detected points
    drawIpoints(img, ipts);

    // Draw the FPS figure
    drawFPS(img);

    // Display the result
    cvShowImage("OpenSURF", img);

    // If ESC key pressed exit loop (find out about bitwise and with 1111 1111...?...why?)
    if( (cvWaitKey(10) & 255) == 27 ) break;
  }

  cvReleaseCapture( &capture );
  cvDestroyWindow( "OpenSURF" );
  return 0;
}


//-------------------------------------------------------

int mainMatch(void)
{
  // Initialise capture device
  CvCapture* capture = cvCaptureFromCAM(1);	//chooses the first USB device-- in this case a shitty webcam
  if(!capture) error("No Capture");		//if there isn't anything in the first USB device, don't continue;

  // Declare matches vectors for each image
  IpPairVec matches;
  IpPairVec matches2;
  IpPairVec matches3;
  // Declare ipts  and reference-ipts for each image
  IpVec ipts, ref_ipts;
  IpVec ipts2, ref_ipts2;
  IpVec ipts3, ref_ipts3;
  
  int check1=0;

//intialize values here
  st_vars.Total = 0;
  st_vars.centerX = 0; 
  st_vars.centerY = 0;
  st_vars.LX=0; 
  st_vars.RX=0;
  st_vars.BY=0;
  st_vars.TY=0;
  st_vars.radius = 0;
  st_vars.radiusHold = 0;
  st_vars.circleCount = 0;
  st_vars.radiusFinal = 0;
  st_vars.centerX_Hold = 0;
  st_vars.centerY_Hold = 0;
  st_vars.centerX_Fin = 0;
  st_vars.centerY_Fin = 0;
  
  st_vars.b = 0; 
  st_vars.a = .45833333333333333; //a = Tan(24.6236 degrees) which is 1/2 the particular viewing angle of this camera
  st_vars.bhold = 0.0;
  st_vars.counter = 0;
  st_vars.counterCenter = 0.0;
  
  // This is the reference object we wish to find in video frame
  // Replace the line below with IplImage *img = cvLoadImage("Images/object.jpg"); 
  // where object.jpg is the planar object to be located in the video
  IplImage *img = cvLoadImage("Images/Security_Compound.jpg");
  IplImage *img2 = cvLoadImage("Images/Ministry_of_Torture.jpg"); //mine
  IplImage *img3 = cvLoadImage("Images/Cheif_of_Security.jpg"); //mine
  if (img == NULL) error("Need to load reference image in order to run matching procedure");
  CvPoint src_corners[4] = {{0,0}, {img->width,0}, {img->width, img->height}, {0, img->height}}; //corners of entire image
  CvPoint dst_corners[4]; //corners of specific object

  CvFont font; //mine
  cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX|CV_FONT_ITALIC, 1.0,1.0,0,2); //mine

  // Extract reference object Ipoints
  surfDetDes(img, ref_ipts, false, 3, 4, 3, 0.004f);
  drawIpoints(img, ref_ipts);
  showImage(img);
  // Describe all three images, storing data in the ref_ipts variables
  surfDetDes(img2, ref_ipts2, false, 3, 4, 3, 0.004f);
  drawIpoints(img2, ref_ipts2);
  showImage(img2);
  surfDetDes(img3, ref_ipts3, false, 3, 4, 3, 0.004f);
  drawIpoints(img3, ref_ipts3);
  showImage(img3);

  // Create a window 
  cvNamedWindow("OpenSURF", CV_WINDOW_AUTOSIZE );

  // Main capture loop
  while( true ) 
  {
    // Grab frame from the capture source
    img = cvQueryFrame(capture);
     
    // Detect and describe interest points in the frame
    surfDetDes(img, ipts, false, 3, 4, 3, 0.004f);

    // Fill match vector
    getMatches(ipts,ref_ipts,matches); //ipts reps new image D points, ref_ipts# reps static image D points, matches is to be filled.
    getMatches(ipts,ref_ipts2,matches2);
    getMatches(ipts,ref_ipts3,matches3);
	
    // This call finds where the object corners should be in the frame
    if (translateCorners(matches, src_corners, dst_corners)) //via debugging, determined this turns out true every time
    { 
      // Draw box around object
      for(int i = 0; i < 4; i++ )
      {
        CvPoint r1 = dst_corners[i%4];
        CvPoint r2 = dst_corners[(i+1)%4];
        cvLine( img, cvPoint(r1.x, r1.y), //draws lines from one corner to next
          cvPoint(r2.x, r2.y), cvScalar(255,255,255), 3 );
      }

     for (unsigned int i = 0; i < matches.size(); ++i)
        drawIpoint(img, matches[i].first);
     for (unsigned int i = 0; i < matches2.size(); ++i) //additional code for drawing descriptor points on live image for all cases
        drawIpoint(img, matches2[i].first);
     for (unsigned int i = 0; i < matches3.size(); ++i)
        drawIpoint(img, matches3[i].first);
    }
    
//*****MATCHES1***************************************************************************************************************************
    //Here we attempt to state that a particular sign has been found, by way of displaying the info beneath the fps on the screen.
    if(matches.size() >= 6){
        check1 = system("./../arduino/arduino -b 9600 -p /dev/ttyUSB0 -s 1"); //serial command to interrupt servos attached to arduino
        cvPutText (img, "Security Compound", cvPoint(10,55), &font, cvScalar(255,255,0));	
        CompleteOps(matches, &st_vars, img, &font);
    }else if(matches2.size() >= 6){
		check1 = system("./../arduino/arduino -b 9600 -p /dev/ttyUSB0 -s 1");
		cvPutText (img, "Ministry of Torture AHHHHHH!", cvPoint(10,55), &font, cvScalar(255,255,0));
		CompleteOps(matches2, &st_vars, img, &font);
    	}else if(matches3.size() >= 6){
			check1 = system("./../arduino/arduino -b 9600 -p /dev/ttyUSB0 -s 1");
			cvPutText (img, "Chief of Security", cvPoint(10,55), &font, cvScalar(255,255,0));
			CompleteOps(matches3, &st_vars, img, &font);
    		} else{ 
    		
    		if (matches.size()>=2 && matches.size()<6)
    			cvPutText (img, "Investigate", cvPoint(10,75), &font, cvScalar(255,255,0));    
    		if (matches2.size()>=2 && matches2.size()<6)
    			cvPutText (img, "Investigate", cvPoint(10,75), &font, cvScalar(255,255,0));  
    		if (matches3.size()>=2 && matches3.size()<6)
    			cvPutText (img, "Investigate", cvPoint(10,75), &font, cvScalar(255,255,0)); 
    			 	
    		st_vars.radius = 0;
    		
    		} //end of all matching conditionals
    

    // Draw the FPS figure
    drawFPS(img);

    // Display the result
    cvShowImage("OpenSURF", img);

    // If ESC key pressed exit loop (find out about bitwise and with 1111 1111...?...why?)
    if( (cvWaitKey(10) & 255) == 27 ) break;
  }

  // Release the capture device
  cvReleaseCapture( &capture );
  cvDestroyWindow( "OpenSURF" );
  return 0;
}

//-------------------------------------------------------


int mainMotionPoints(void)
{
  // Initialise capture device
  CvCapture* capture = cvCaptureFromCAM( CV_CAP_ANY );
  if(!capture) error("No Capture");

  // Create a window 
  cvNamedWindow("OpenSURF", CV_WINDOW_AUTOSIZE );

  // Declare Ipoints and other stuff
  IpVec ipts, old_ipts, motion;
  IpPairVec matches;
  IplImage *img;

  // Main capture loop
  while( 1 ) 
  {
    // Grab frame from the capture source
    img = cvQueryFrame(capture);

    // Detect and describe interest points in the image
    old_ipts = ipts;
    surfDetDes(img, ipts, true, 3, 4, 2, 0.0004f);

    // Fill match vector
    getMatches(ipts,old_ipts,matches);
    for (unsigned int i = 0; i < matches.size(); ++i) 
    {
      const float & dx = matches[i].first.dx;
      const float & dy = matches[i].first.dy;
      float speed = sqrt(dx*dx+dy*dy);
      if (speed > 5 && speed < 30) 
        drawIpoint(img, matches[i].first, 3);
    }
        
    // Display the result
    cvShowImage("OpenSURF", img);

    // If ESC key pressed exit loop (find out about bitwise and with 1111 1111...?...why?)
    if( (cvWaitKey(10) & 255) == 27 ) break;
  }

  // Release the capture device
  cvReleaseCapture( &capture );
  cvDestroyWindow( "OpenSURF" );
  return 0;
}


//-------------------------------------------------------

int mainStaticMatch()
{
  IplImage *img1, *img2;
  img1 = cvLoadImage("Images/img1.jpg");
  img2 = cvLoadImage("Images/img2.jpg");

  IpVec ipts1, ipts2;
  surfDetDes(img1,ipts1,false,4,4,2,0.0001f);
  surfDetDes(img2,ipts2,false,4,4,2,0.0001f);

  IpPairVec matches;
  getMatches(ipts1,ipts2,matches);

  for (unsigned int i = 0; i < matches.size(); ++i)
  {
    drawPoint(img1,matches[i].first);
    drawPoint(img2,matches[i].second);
  
    const int & w = img1->width;
    cvLine(img1,cvPoint(matches[i].first.x,matches[i].first.y),cvPoint(matches[i].second.x+w,matches[i].second.y), cvScalar(255,255,255),1);
    cvLine(img2,cvPoint(matches[i].first.x-w,matches[i].first.y),cvPoint(matches[i].second.x,matches[i].second.y), cvScalar(255,255,255),1);
  }

  std::cout<< "Matches: " << matches.size();

  cvNamedWindow("1", CV_WINDOW_AUTOSIZE );
  cvNamedWindow("2", CV_WINDOW_AUTOSIZE );
  cvShowImage("1", img1);
  cvShowImage("2",img2);
  cvWaitKey(0);

  return 0;
}

//-------------------------------------------------------

int mainKmeans(void)
{
  IplImage *img = cvLoadImage("Images/img1.jpg");
  IpVec ipts;
  Kmeans km;
  
  // Get Ipoints
  surfDetDes(img,ipts,true,3,4,2,0.0006f);

  for (int repeat = 0; repeat < 10; ++repeat)
  {

    IplImage *img = cvLoadImage("Images/img1.jpg");
    km.Run(&ipts, 5, true);
    drawPoints(img, km.clusters);

    for (unsigned int i = 0; i < ipts.size(); ++i)
    {
      cvLine(img, cvPoint(ipts[i].x,ipts[i].y), cvPoint(km.clusters[ipts[i].clusterIndex].x ,km.clusters[ipts[i].clusterIndex].y),cvScalar(255,255,255));
    }

    showImage(img);
  }

  return 0;
}



